classdef doubleJointControler  < handle
    %这个用的是增量式pid控制器
    properties
        u_now;     % 控制量，成为真正的控制输出
        ref_value;   %这个就是参考值，然后减去实际值就是err
        err_sum;    %求和的误差，用于增量式
        err_last;
        
        force_err_sum; %用于力控制器
        
        %params
        kp;
        kv;
        tv;
        tm;
        km;
        kd;
        kt;
        
        
        wn;
        zeta;
        %inverse_dynamic_params
        pid_kp;
        pid_ki;
        pid_kd;
        
        
        pid_kfp;
        pid_kfv;
        pid_kfi;
        
        resist_kp;%需要调用update函数更新
        resist_kv;
        virtual_mass;
        
        
        is_int_enable;
        is_speed_target_enable;
        is_speed_ff_enable;
        is_acc_ff_enable;
        is_torque_ff_enable;
        
        n_group;
    end
    
    methods
        function obj = doubleJointControler(sj_obj,motor_obj,ts,zeta,enable_vector)
            if(nargin >= 2 )
                obj.n_group = sj_obj.n_group;
                %先用sj初始化所有参数
                if(nargin >= 4)
                    obj.zeta = zeta;
                    obj.wn = zeros(1,1);
                    rows = find(zeta(:,1)== 1,obj.n_group);
                    obj.wn(rows,1) = 4.75./ts(rows,1);
                    rows = find(zeta(:,1)> 1,obj.n_group);
                    obj.wn(rows,1) = 3.3./((zeta(rows,1)- sqrt(zeta(rows,1).^2-1)).*ts(rows,1));
                    rows = find(zeta(:,1)< 1,obj.n_group);
                    obj.wn(rows,1) =  3.5./(zeta(rows,1).*ts(rows,1));  
                else
                    %使用默认的特性
                    ts = repmat(0.1,1,1);%调节时间
                    obj.zeta = ones(1,1);%阻尼比
                    obj.wn = 4.75./ts;
                end
                
                %以下全部使用力矩模式的值，因为题目如此要求
                obj.kt = motor_obj.Ka*motor_obj.Kg;
                obj.tm = sj_obj.f_Im /(sj_obj.Bm); % 用理论等效惯量计算
                obj.km = obj.kt.*inv(sj_obj.Bm);
                obj.kd = inv(sj_obj.Bm);
                
                obj.tv = obj.tm;
                obj.kv = 2*zeta.*obj.wn.* inv(obj.km);
                obj.kp = obj.wn./(2*zeta);
                
                obj.pid_kp = (obj.wn).^2;
                obj.pid_kd = 2.*obj.wn.*zeta;
                obj.pid_ki = repmat(5,obj.n_group,1);
                

                disp("using default attribute");
                [obj.u_now,obj.err_last,obj.err_sum,...
                    obj.is_int_enable,obj.is_speed_target_enable,...
                    obj.is_speed_ff_enable,obj.is_acc_ff_enable,...
                    obj.is_torque_ff_enable,obj.force_err_sum,...
                    obj.resist_kp,obj.resist_kv]...
                    = deal(zeros(obj.n_group,1));
                if(nargin == 5)
                    tmp = mat2cell(enable_vector,[2],ones(5,1));
                    [obj.is_int_enable,obj.is_speed_target_enable,...
                    obj.is_speed_ff_enable,obj.is_acc_ff_enable,...
                    obj.is_torque_ff_enable] = deal(tmp{:});
                end
            else
                ME = MException('need at least 2  param!');
                throw(ME)
            end
            
            %预设好力控制器的pid值
            obj.pid_kfp = 150;
            obj.pid_kfi = 10;
            obj.pid_kfv = [100;3500];
            

            
            
        end
        
        
        function pid_reset(obj)
            [obj.u_now,obj.err_last,obj.err_sum] = deal(zeros(obj.n_group,1));
        end
       
        function resist_pid_update(obj,K,m_v_x,st_pos,ed_pos,ref_pos)
            obj.resist_kp(1) = m_v_x* (obj.wn(1)^2);
            obj.resist_kv(1) = 2*obj.zeta(1)*obj.resist_kp(1)/obj.wn(1);
            obj.resist_kp(2) = K*(ed_pos-st_pos)/(ref_pos-ed_pos);
            obj.virtual_mass = [m_v_x 0 ;...
               0  (obj.resist_kp(2)+K)/(obj.wn(2)^2)];
            obj.resist_kv(2) = 2*obj.zeta(2)*(obj.resist_kp(2)+K)/obj.wn(2); 
        end
        
        function con_value = get_con_value(obj,dj_obj,simu_period)
            actual_value = dj_obj.joint_state;
             %这个是获得累加式的控制值，每次要传入state-info，也就是三种量
            con_value = zeros(obj.n_group,1,4);%组数、一行、con, e_pos, err_vel,e_sum
            err_now = (obj.ref_value - actual_value).*dj_obj.n_factor;
            %con始终为列向量
            pos_con = obj.kp .* err_now(:,1);
            
            vel_err_now = pos_con+ err_now(:,2).*obj.is_speed_target_enable...
                - actual_value(:,2).*dj_obj.n_factor.*(~obj.is_speed_target_enable);
            obj.err_sum = obj.err_sum + vel_err_now * simu_period;
            
            obj.u_now = obj.kv*(obj.tv * vel_err_now) + (obj.kv * obj.err_sum).*obj.is_int_enable;       
            %还需要增加前馈部分
%             obj.u_now = obj.u_now + (obj.km\obj.ref_value(:,2)).*obj.is_speed_ff_enable  +( (obj.tm/(obj.km))  * obj.ref_value(:,3)).*obj.is_acc_ff_enable...
%                 + obj.is_torque_ff_enable.*torque./obj.kt;
% %             
%             obj.u_now = obj.u_now + (obj.km\obj.ref_value(:,2)).*obj.is_speed_ff_enable...
%                 +( (obj.tm*inv(obj.km))  * obj.ref_value(:,3)).*obj.is_acc_ff_enable...
%                 ./(obj.kt*50) + obj.is_torque_ff_enable.*torque./obj.kt;
%             

%             obj.u_now = obj.u_now.*50 + obj.is_torque_ff_enable.*torque./obj.kt;
            
            
            

            [M,V,G] = dj_obj.inverse_dynamic_matrix(1,dj_obj.joint_state); %转子
            tmp = obj.ref_value.*dj_obj.n_factor;
            calculated_torque = (M * tmp(:,3).*obj.is_acc_ff_enable  +...
                (G.*dj_obj.is_g_enable + V*tmp(:,2)).*obj.is_torque_ff_enable...
                + dj_obj.Bm *tmp(:,2).*obj.is_speed_ff_enable ); %转子空间的
            obj.u_now = obj.u_now + calculated_torque./obj.kt;
            
            %输出的直接往关节去了
            con_value(:,1,3) =  obj.err_last;
            obj.err_last = vel_err_now;
            con_value(:,1,1) = obj.u_now;
            con_value(:,1,2) = vel_err_now;
            con_value(:,1,4) = obj.err_sum;

        end
        
        
        function con_value = get_id_con_value(obj,dj_obj,motor_obj,simu_period)
            %这个是逆动力学控制器，假定用电机的力矩模式了，算出fc直接除系数
           %实际值还是传n*3
            con_value = zeros(obj.n_group,4);%组数、一行、con, e_pos, err_vel,e_sum
            err_now = obj.ref_value - dj_obj.joint_state;
            
            obj.err_sum = obj.err_sum + err_now(:,1).*simu_period;
            acc_design = obj.ref_value(:,3)+ obj.pid_kp.*err_now(:,1)...
                +(obj.pid_ki.*obj.err_sum(:,1)).*(obj.is_int_enable)+ ...
                obj.pid_kd.*(err_now(:,2));
            
            %旧用的逆动力学有问题，重开
%             tmp = dj_obj.inverse_dynamic(1,dj_obj.joint_state,[dj_obj.state(:,1:2) acc_design.*dj_obj.n_factor]);
%             con_value(:,1) = (tmp(:,1)+tmp(:,2)-tmp(:,3))./(motor_obj.Ka*motor_obj.Kg);
%             
            %重开内容,MVG得到的是转子空间的值，但是进行design的是关节空间的量，需要转换
            [M,V,G] = dj_obj.inverse_dynamic_matrix(1,dj_obj.joint_state);
            con_value(:,1) = (M * acc_design .*dj_obj.n_factor +...
                G.*dj_obj.is_g_enable + V*dj_obj.state(:,2) + dj_obj.Bm *dj_obj.state(:,2) )...
                ./(motor_obj.Ka*motor_obj.Kg);
            

            %输出限幅
            bad_rows = find(con_value(:,1)< -10,dj_obj.n_group);
            con_value(bad_rows,1) = -10;
            bad_rows = find(con_value(:,1)> 10,dj_obj.n_group);
            con_value(bad_rows,1) = 10;
            
            con_value(:,2) = err_now(:,1);
            con_value(:,3) = err_now(:,2);
            con_value(:,4) = err_now(:,3);          
        end
        
        
        function con_torque = get_pos_tor_blend_con_value(obj,dj_obj,cart_value,fe,simu_period)
            %这里直接输出了力矩给关节了，不经过电机空间，反正是力矩模式
            %这里传的fe是指的实际的环境接触力
            jacobi_m = dj_obj.get_jacobi(1);
            d_jacobi_m = dj_obj.get_dot_jacobi(1);
            err_now = obj.ref_value- [cart_value,[0;0],fe];

            [M,V,B,G] = dj_obj.joint_inverse_dynamic_matrix(1,dj_obj.joint_state);
            M_X = inv(jacobi_m)' * M * inv(jacobi_m);
            N_X = inv(jacobi_m)' * (V+ B -M*inv(jacobi_m)*d_jacobi_m)*inv(jacobi_m);
            G_X = inv(jacobi_m)' * G;
            
            %这个就是要准备计算施加给关节的末端需要的控制力
            f_cart_c = N_X*cart_value(:,2)+G_X;
            
            obj.err_sum = obj.err_sum + err_now(:,1)*simu_period;
            %先来位控
            acc_designed = obj.ref_value(:,3)+ err_now(:,1).*obj.pid_kp...
                + err_now(:,2).*obj.pid_kd + obj.err_sum.*obj.pid_ki.*obj.is_int_enable;
            cp  = [1 ;1]; %直接用点乘了
            f_cart_c = f_cart_c + cp .* (M_X * acc_designed);
            

            %再来力控
            
            %这里是力的误差积分项

            cf = [0;1];
            obj.force_err_sum = obj.force_err_sum + err_now(:,4)*simu_period;
            f_cart_c = f_cart_c - cf.*(obj.ref_value(:,4)+  err_now(:,4) .*obj.pid_kfp...
                +obj.force_err_sum .*obj.pid_kfi + cart_value(:,2).*obj.pid_kfv);
            %现在根据末端力反求转子力矩
            %前面按照真实受力来算
            con_torque = (jacobi_m' * (f_cart_c+fe))./dj_obj.n_factor;
        end
        
        
         function con_torque = get_resist_con_value(obj,dj_obj,cart_value,fe)
            %这里直接输出了力矩给关节了，不经过电机空间，反正是力矩模式
            %这里传的fe是指的实际的环境接触力
            jacobi_m = dj_obj.get_jacobi(1);
            d_jacobi_m = dj_obj.get_dot_jacobi(1);
            err_now = obj.ref_value- [cart_value,[0;0],fe];

            [M,V,B,G] = dj_obj.joint_inverse_dynamic_matrix(1,dj_obj.joint_state);
            M_X = inv(jacobi_m)' * M * inv(jacobi_m);
            N_X = inv(jacobi_m)' * (V+ B -M*inv(jacobi_m)*d_jacobi_m)*inv(jacobi_m);
            G_X = inv(jacobi_m)' * G;
            
            %这个就是要准备计算施加给关节的末端需要的控制力
            f_cart_c = N_X*cart_value(:,2)+G_X;
            
            acc_designed  = obj.ref_value(:,3)+ (obj.virtual_mass)\...
                (fe+ obj.resist_kp.*err_now(:,1) + obj.resist_kv.*err_now(:,2))...
                ;
            f_cart_c = f_cart_c + M_X*acc_designed - fe;
            
            %现在根据末端力反求转子力矩
            con_torque = (jacobi_m' * (f_cart_c+fe))./dj_obj.n_factor;
        end
        
        
        function set_target(obj,ref_value)
            obj.ref_value = ref_value;            
        end
    end
     
end